#include "Code1_MOTOR.h"
#include "Code6_HEAD.h"
#include <math.h>
#include <stdlib.h>

HeadMotionStruct Head;
Bool HeadSearchBack = FALSE;
				 
extern MotorDataStruct MotorData;			
extern HeadScheduleStruct HeadSchedule; 
extern MotionStruct Motion;

int HeadSearchPoint[SEARCH_POINT_NUM+1][2] = { {512, 512},	 // NONE_HEAD / Initial (512,512)
																						 	 {512, 256},   // RU (512,256)
                                            	 {512, 768},   // LU (512,768)
                                             	 {640, 256},   // RC (640,256)
                                             	 {640, 768},   // LC (640,768)
                                             	 {768, 358},   // RL (768,256)
                                             	 {768, 666}};  // LL (768,768)     

HeadStateDef Upper[2] = {RU, LU};		 
HeadStateDef Center[2] = {RC, LC};			 
HeadStateDef Lower[2] = {RL, LL};					 
HeadStateDef Upper_Center[4] = {RU, LU, LC, RC};		
HeadStateDef Half_R[4] = {RU, RC, RL, RC};					
HeadStateDef Half_L[4] = {LU, LC, LL, LC};	 				
HeadStateDef Radiation_R[8] = {RU, LU, LC, RC, RL, LL, LC, RC};
HeadStateDef Radiation_L[8] = {LU, RU, RC, LC, LL, RL, RC, LC};

void Head_Init(void)
{
	Head.Mode = NO_MOTION;	 
	Head.Mode_RX = NO_MOTION;		
	Head.Mode_Past = NO_MOTION;

	Head.Pos[0][0] = 0;				 
	Head.Pos[0][1] = 0;
	Head.Pos[1][0] = 0;
	Head.Pos[1][1] = 0;

	Head.StateMode = NONE_HEAD;	 	
	Head.StateIndex = 0;
}

void HeadMotion(void)
{
	int head_0_now = 	MotorData.Read[0][READ_LOW_POSITION] + (MotorData.Read[0][READ_HIGH_POSITION] << 8);
	int head_1_now = 	MotorData.Read[1][READ_LOW_POSITION] + (MotorData.Read[1][READ_HIGH_POSITION] << 8);
	
	if(Head.Mode_Past != Head.Mode)
	{
		Head.StateMode = NONE_HEAD;
		Head.StateIndex = 0;
	}

	switch(Head.Mode)
	{
		case HEAD_STOP:	
      HeadSchedule.NowIndex = 0;
      HeadSchedule.EndIndex = 0;
      HeadSchedule.Delay_ExecutedCount = 0;
      Add_head_job(253,6,0,0,0); 
      Add_head_job(0, MotorData.Read[0][READ_LOW_POSITION], MotorData.Read[0][READ_HIGH_POSITION], 128, 0);
      Add_head_job(1, MotorData.Read[1][READ_LOW_POSITION], MotorData.Read[1][READ_HIGH_POSITION], 128, 0);        
      Head.Mode = NO_MOTION;																																										    
      Head.StateMode = NONE_HEAD;							
		break;
		
		case HEAD_GO_SLOW: 
      HeadSchedule.NowIndex = 0;
      HeadSchedule.EndIndex = 0;
      HeadSchedule.Delay_ExecutedCount = 0;
      Add_head_job(253,6,0,0,0); 
      Add_head_job(0, Head.Pos[0][LOW], Head.Pos[0][HIGH], 128, 0);
      Add_head_job(1, Head.Pos[1][LOW], Head.Pos[1][HIGH], 128, 0);         
      Head.Mode = NO_MOTION;																																										         
      Head.StateMode = NONE_HEAD;																								    
      Head.StateIndex = 0;		
		break;
		
		case HEAD_GO_MIDDLE:
      HeadSchedule.NowIndex = 0;
      HeadSchedule.EndIndex = 0;
      HeadSchedule.Delay_ExecutedCount = 0;
      Add_head_job(253,6,0,0,0); 
      Add_head_job(0, Head.Pos[0][LOW], Head.Pos[0][HIGH], 0, 1);
      Add_head_job(1, Head.Pos[1][LOW], Head.Pos[1][HIGH], 0, 1);      
      Head.Mode = NO_MOTION;																																										            
      Head.StateMode = NONE_HEAD;																					    
      Head.StateIndex = 0;				
		break;
		
		case HEAD_GO_FAST:
      HeadSchedule.NowIndex = 0;
      HeadSchedule.EndIndex = 0;
      HeadSchedule.Delay_ExecutedCount = 0;
      Add_head_job(253,6,0,0,0); 
      Add_head_job(0, Head.Pos[0][LOW], Head.Pos[0][HIGH], 0, 2);
      Add_head_job(1, Head.Pos[1][LOW], Head.Pos[1][HIGH], 0, 2);      
      Head.Mode = NO_MOTION;																								    
      Head.StateMode = NONE_HEAD;																						    
      Head.StateIndex = 0;		
		break;
		
		case HEAD_SEARCH_UPPER:
      if((HeadSchedule.NowIndex == HeadSchedule.EndIndex) && (HeadSchedule.Delay_ExecutedCount==0)) {
        HeadSearch(head_0_now, head_1_now, HAED_SEARACH_SPEED_L, HAED_SEARACH_SPEED_H, 0, Upper, sizeof(Upper)); 
      }															
		break;
		
		case HEAD_SEARCH_CENTER:	
      if((HeadSchedule.NowIndex == HeadSchedule.EndIndex) && (HeadSchedule.Delay_ExecutedCount==0)) {
        HeadSearch(head_0_now, head_1_now, HAED_SEARACH_SPEED_L, HAED_SEARACH_SPEED_H, 0, Center, sizeof(Center));         
      }	 			
		break;
		
		case HEAD_SEARCH_LOWER:
      if((HeadSchedule.NowIndex == HeadSchedule.EndIndex) && (HeadSchedule.Delay_ExecutedCount==0)) {
        HeadSearch(head_0_now, head_1_now, HAED_SEARACH_SPEED_L, HAED_SEARACH_SPEED_H, 0, Lower, sizeof(Lower));         
      }	 			
		break;
		
		case HEAD_SEARCH_UPPER_CENTER:
      if((HeadSchedule.NowIndex == HeadSchedule.EndIndex) && (HeadSchedule.Delay_ExecutedCount==0)) {
        HeadSearch(head_0_now, head_1_now, HAED_SEARACH_SPEED_L, HAED_SEARACH_SPEED_H, 0, Upper_Center, sizeof(Upper_Center));         
      }	 			
		break;
		
		case HEAD_SEARCH_HALF_R:
      if((HeadSchedule.NowIndex == HeadSchedule.EndIndex) && (HeadSchedule.Delay_ExecutedCount==0)) {
        HeadSearch(head_0_now, head_1_now, HAED_SEARACH_SPEED_L, HAED_SEARACH_SPEED_H, 0, Half_R, sizeof(Half_R));         
      }	 			
		break;
		
		case HEAD_SEARCH_HALF_L:
      if((HeadSchedule.NowIndex == HeadSchedule.EndIndex) && (HeadSchedule.Delay_ExecutedCount==0)) {
        HeadSearch(head_0_now, head_1_now, HAED_SEARACH_SPEED_L, HAED_SEARACH_SPEED_H, 0, Half_L, sizeof(Half_L));         
      }	 			
		break;
		
		case HEAD_SEARCH_RADIATION_R:
      if((HeadSchedule.NowIndex == HeadSchedule.EndIndex) && (HeadSchedule.Delay_ExecutedCount==0)) {
        HeadSearch(head_0_now, head_1_now, HAED_SEARACH_SPEED_L, HAED_SEARACH_SPEED_H, 0, Radiation_R, sizeof(Radiation_R));         
      }	 			
		break;									 

		case HEAD_SEARCH_RADIATION_L:
      if((HeadSchedule.NowIndex == HeadSchedule.EndIndex) && (HeadSchedule.Delay_ExecutedCount==0)) {
        HeadSearch(head_0_now, head_1_now, HAED_SEARACH_SPEED_L, HAED_SEARACH_SPEED_H, 0, Radiation_L, sizeof(Radiation_L));         
      }	 			
		break;

		default:											 
      Head.Mode = NO_MOTION;
      Head.StateMode = NONE_HEAD;		
      Head.StateIndex = 0;
    break;
	}
	
	Head.Mode_Past = Head.Mode;
}																							 
			
void HeadSearch(int head_0_now, int head_1_now, unsigned char SpeedL, unsigned char SpeedH, unsigned char head_wait_count, HeadStateDef *Type, int size)
{
	if(Head.StateMode == NONE_HEAD)
	{
		Head.StateIndex = CheckHeadPosition(head_0_now, head_1_now, Type, size);
		Head.StateMode = Type[Head.StateIndex];
	}
	ScanByHead(SpeedL, SpeedH, head_wait_count);

	if((abs(head_0_now - (HeadSearchPoint[Head.StateMode][0] + MotorData_Correction[0])) <= 10) &&
		 (abs(head_1_now - (HeadSearchPoint[Head.StateMode][1] + MotorData_Correction[1])) <= 10))
	{
		Head.StateIndex = (Head.StateIndex + 1) % size;
	  Head.StateMode = Type[Head.StateIndex];
	}
}
							
int CheckHeadPosition(int head_0_now, int head_1_now, HeadStateDef *Type, int size) 
{
	int i;									 
	int nearest_point_index = 0;
  unsigned int distance_temp = 0;
  unsigned int distance_min = 8192;
  int head_0_temp = 0;
  int head_1_temp = 0;
	int length = (size > SEARCH_POINT_NUM) ? SEARCH_POINT_NUM : size;

	for(i=0; i < length; i++)
	{
		head_0_temp = HeadSearchPoint[Type[i]][0];		 
		head_1_temp = HeadSearchPoint[Type[i]][1];	
    distance_temp = sqrt(pow(head_0_now-head_0_temp,2) + pow(head_1_now-head_1_temp,2));

    if(distance_temp < distance_min) 
		{
      distance_min = distance_temp;
      nearest_point_index = i;
    }    
	}
	return nearest_point_index;
}

void ScanByHead(unsigned char SpeedL, unsigned char SpeedH, unsigned char head_wait_count) {
  unsigned char head_0_temp_L = HeadSearchPoint[Head.StateMode][0] & 0x00ff;
  unsigned char head_0_temp_H = (HeadSearchPoint[Head.StateMode][0] & 0xff00) >> 8;
  unsigned char head_1_temp_L = HeadSearchPoint[Head.StateMode][1] & 0x00ff;
  unsigned char head_1_temp_H = (HeadSearchPoint[Head.StateMode][1] & 0xff00) >> 8;
  Add_head_job(253,6,0,0,0); 
  Add_head_job(0, head_0_temp_L, head_0_temp_H, SpeedL, SpeedH);
  Add_head_job(1, head_1_temp_L, head_1_temp_H, SpeedL, SpeedH);
  /*
  if(head_wait_count>0) {
    Add_head_job(  255,   head_wait_count,   0,   0,   0 );
  }
  */
}
//
//void Head_Search_Upper(int head_0_now, int head_1_now, unsigned char SpeedL, unsigned char SpeedH, unsigned char head_wait_count) {
//  if(HeadStateMode == 0) {
//    Check_Head_position(head_0_now, head_1_now, RU, LU, 1);
//  }
//  Scan_By_Head(SpeedL, SpeedH, head_wait_count);
//  
//  if((abs(head_0_now-head_search_point[HeadStateMode][0])<10) && (abs(head_1_now-head_search_point[HeadStateMode][1])<10))
//  {
//    switch(HeadStateMode) {
//      case RU:      
//        HeadStateMode = LU;
//        HeadSearchBack = FALSE;
//        break;
//      /*
//      case CU:      
//        if(HeadSearchBack == TRUE) {
//          HeadStateMode = RU;
//        }
//        else {
//          HeadStateMode = LU;
//        }
//        break;  
//      */
//      case LU:
//        HeadStateMode = RU;
//        HeadSearchBack = TRUE;
//        break;
//      default:
//        HeadStateMode = NONE_HEAD;
//        break;      
//    }
//  }
//}
//
//void Head_Search_Center(int head_0_now, int head_1_now, unsigned char SpeedL, unsigned char SpeedH, unsigned char head_wait_count) {
//  if(HeadStateMode == 0) {
//    Check_Head_position(head_0_now, head_1_now, RC, LC, 1);
//  }
//  Scan_By_Head(SpeedL, SpeedH, head_wait_count);
//  
//  if((abs(head_0_now-head_search_point[HeadStateMode][0])<10) && (abs(head_1_now-head_search_point[HeadStateMode][1])<10))
//  {
//    switch(HeadStateMode) {
//      case RC:
//        HeadStateMode = LC;
//        HeadSearchBack = FALSE;
//        break;
//      /*
//      case CC:      
//        if(HeadSearchBack == TRUE) {
//          HeadStateMode = RC;
//        }
//        else {
//          HeadStateMode = LC;
//        }
//        break;  
//      */
//      case LC:
//        HeadStateMode = RC;
//        HeadSearchBack = TRUE;
//        break;
//      default:
//        HeadStateMode = NONE_HEAD;
//        break;      
//    }
//  }
//}
//
//void Head_Search_Lower(int head_0_now, int head_1_now, unsigned char SpeedL, unsigned char SpeedH, unsigned char head_wait_count) {
//  if(HeadStateMode == 0) {
//    Check_Head_position(head_0_now, head_1_now, RL, LL, 1);
//  }
//  Scan_By_Head(SpeedL, SpeedH, head_wait_count);
//  
//  if((abs(head_0_now-head_search_point[HeadStateMode][0])<10) && (abs(head_1_now-head_search_point[HeadStateMode][1])<10))
//  {
//    switch(HeadStateMode) {
//      case RL:   
//        HeadStateMode = LL;
//        HeadSearchBack = FALSE;
//        break;
//      /*
//      case CL:      
//        if(HeadSearchBack == TRUE) {
//          HeadStateMode = RL;
//        }
//        else {
//          HeadStateMode = LL;
//        }
//        break;  
//      */
//      case LL:
//        HeadStateMode = RL;
//        HeadSearchBack = TRUE;
//        break;
//      default:
//        HeadStateMode = NONE_HEAD;
//        break;      
//    }
//  }
//}
//
//void Head_Search_Upper_Center(int head_0_now, int head_1_now, unsigned char SpeedL, unsigned char SpeedH, unsigned char head_wait_count) {
//  if(HeadStateMode == 0) {
//    Check_Head_position(head_0_now, head_1_now, RU, LC, 1);
//  }
//  Scan_By_Head(SpeedL, SpeedH, head_wait_count);
//  
//  if((abs(head_0_now-head_search_point[HeadStateMode][0])<10) && (abs(head_1_now-head_search_point[HeadStateMode][1])<10))
//  {
//    switch(HeadStateMode) {
//      case RU:   
//        HeadStateMode = LU;
//        break;
//      //case CU:      
//      //  HeadStateMode = LU;
//      //  break;
//      case LU:
//        HeadStateMode = LC;
//        break;
//      case LC:
//        HeadStateMode = RC;
//        break;
//      //case CC:      
//      //  HeadStateMode = RC;
//      //  break;
//      case RC:
//        HeadStateMode = RU;
//        break;
//      default:
//        HeadStateMode = NONE_HEAD;
//        break;      
//    }
//  }
//}
//void Head_Search_Half_R(int head_0_now, int head_1_now, unsigned char SpeedL, unsigned char SpeedH, unsigned char head_wait_count) {
//  if(HeadStateMode == 0) {
//    Check_Head_position(head_0_now, head_1_now, RU, RL, 2);
//  }
//  Scan_By_Head(SpeedL, SpeedH, head_wait_count);
//  
//  if((abs(head_0_now-head_search_point[HeadStateMode][0])<10) && (abs(head_1_now-head_search_point[HeadStateMode][1])<10))
//  {
//   switch(HeadStateMode) {
//     case RU:      
//       HeadStateMode = RC;
//       HeadSearchBack = FALSE;
//       break;
//     case RC:  
//       if(HeadSearchBack == TRUE) {
//         HeadStateMode = RU;
//       }
//       else {
//         HeadStateMode = RL;
//       }
//       break;
//     case RL:
//       HeadStateMode = RC;
//       HeadSearchBack = TRUE;
//       break;
//     default:
//       HeadStateMode = NONE_HEAD;
//       break;  
//     }
//  }
//}
//void Head_Search_Half_L(int head_0_now, int head_1_now, unsigned char SpeedL, unsigned char SpeedH, unsigned char head_wait_count) {
//  if(HeadStateMode == 0) {
//    Check_Head_position(head_0_now, head_1_now, LU, LL, 2);
//  }
//  Scan_By_Head(SpeedL, SpeedH, head_wait_count);
//  
//  if((abs(head_0_now-head_search_point[HeadStateMode][0])<10) && (abs(head_1_now-head_search_point[HeadStateMode][1])<10))
//  {
//    switch(HeadStateMode) {
//      case LU:      
//        HeadStateMode = LC;
//        HeadSearchBack = FALSE;
//        break;
//      case LC:  
//        if(HeadSearchBack == TRUE) {
//          HeadStateMode = LU;
//        }
//        else {
//          HeadStateMode = LL;
//        }
//        break;
//      case LL:
//        HeadStateMode = LC;
//        HeadSearchBack = TRUE;
//        break;
//      default:
//        HeadStateMode = NONE_HEAD;
//        break; 
//    }
//  }
//}
//void Head_Search_Radiation_R(int head_0_now, int head_1_now, unsigned char SpeedL, unsigned char SpeedH, unsigned char head_wait_count) {
//  if(HeadStateMode == 0) {
//    Check_Head_position(head_0_now, head_1_now, RU, LL, 1);
//  }
//  Scan_By_Head(SpeedL, SpeedH, head_wait_count);
//  
//  if((abs(head_0_now-head_search_point[HeadStateMode][0])<10) && (abs(head_1_now-head_search_point[HeadStateMode][1])<10))
//  {
//    switch(HeadStateMode) {
//      case RU:  
//        HeadStateMode = LU;
//        HeadSearchBack = FALSE;
//        break;
//     //case CU:      
//     //   HeadStateMode = LU;
//     //   break;
//     case LU:
//        HeadStateMode = LC;
//        break;
//     case LC:
//        HeadStateMode = RC;
//        break;
//     //case CC:  
//     //   HeadStateMode = RC;
//     //   break;
//     case RC:
//        if(HeadSearchBack == TRUE) {
//          HeadStateMode = RU;
//        }
//        else {
//        HeadStateMode = RL;
//        }
//        break;
//     case RL:      
//        HeadStateMode = LL;
//        break;
//     //case CL:      
//     //   HeadStateMode = LL;
//     //   break;
//     case LL:
//        HeadStateMode = LC;
//        HeadSearchBack = TRUE;
//        break;
//     default:
//        HeadStateMode = NONE_HEAD;
//        break;      
//    }
//  }
//}
//void Head_Search_Radiation_L(int head_0_now, int head_1_now, unsigned char SpeedL, unsigned char SpeedH, unsigned char head_wait_count) {
//  if(HeadStateMode == 0) {
//    Check_Head_position(head_0_now, head_1_now, RU, LU, 1);
//  }
//  Scan_By_Head(SpeedL, SpeedH, head_wait_count);
//  
//  if((abs(head_0_now-head_search_point[HeadStateMode][0])<10) && (abs(head_1_now-head_search_point[HeadStateMode][1])<10))
//  {
//    switch(HeadStateMode) {
//      case LU:      
//        HeadStateMode = RU;
//        HeadSearchBack = FALSE;
//        break;
//     //case CU:      
//     //   HeadStateMode = RU;
//     //   break;
//     case RU:
//        HeadStateMode = RC;
//        break;
//     case RC:
//        HeadStateMode = LC;
//        break;
//     //case CC:  
//     //   HeadStateMode = LC;
//     //   break;
//     case LC:
//        if(HeadSearchBack == TRUE) {
//          HeadStateMode = LU;
//        }
//        else {
//          HeadStateMode = LL;
//        }
//        break;
//     case LL:      
//        HeadStateMode = RL;
//        break;
//     //case CL:      
//     //   HeadStateMode = RL;
//     //   break;
//     case RL:
//        HeadStateMode = RC;
//        HeadSearchBack = TRUE;
//        break;
//     default:
//        HeadStateMode = NONE_HEAD;
//        break;      
//    }
//  }
//}     
